//
// Created by 26757 on 25-5-23.
//

#include "Chassis.h"
#include "numbers"

using namespace std;

bool Chassis::initialize() {
    for (auto& servo : servos) {
        if (!servo.initialize()) {
            return false;
        }
    }
    initialized = true;
    return true;
}

bool Chassis::enable() {
    if (!initialized) return false;
    for (auto& servo : servos) {
        if (!servo.enable()) {
            return false;
        }
    }
    enabled = true;
    return true;
}

bool Chassis::disable() {
    if (!initialized) return false;
    for (auto& servo : servos) {
        if (!servo.disable()) {
            return false;
        }
    }
    enabled = false;
    return true;
}

bool Chassis::start() {
    if (!initialized || !enabled) return false;
    for (auto& servo : servos) {
        if (!servo.start()) {
            return false;
        }
    }
    started = true;
    return true;
}

bool Chassis::stop() {
    if (!initialized || !enabled) return false;
    for (auto& servo : servos) {
        if (!servo.stop()) {
            return false;
        }
    }
    started = false;
    return true;
}

void Chassis::ctrl() {
    if (!started) return;
    if (first_run) {
        for (int i = 0; i < 2; ++i) {
            target_angles[i] = servos[i].motor.angle;
        }
        first_run = false;
    }

    float tmp_angles[2];
    constexpr float r = 0.0272f; // 轮子半径,单位:m // 非真实值

    constexpr float kz = 2.45f; // 校准系数,测量得到
    // m/s to rpm
    tmp_angles[0] = speed_y / (2 * numbers::pi_v<float> * r) * 60 - kz * speed_z;
    tmp_angles[1] = -speed_y / (2 * numbers::pi_v<float> * r) * 60 - kz * speed_z;

    // rpm to rad/s
    tmp_angles[0] = tmp_angles[0] * 2 * numbers::pi_v<float> / 60;
    tmp_angles[1] = tmp_angles[1] * 2 * numbers::pi_v<float> / 60;

    // rad/s to ...
    target_angles[0] += tmp_angles[0] / freq;
    target_angles[1] += tmp_angles[1] / freq;

    // limit
    for (int i = 0; i < 2; ++i) {
        if (target_angles[i] > 2 * numbers::pi_v<float>) {
            target_angles[i] -= 2 * numbers::pi_v<float>;
        } else if (target_angles[i] < -2 * numbers::pi_v<float>) {
            target_angles[i] += 2 * numbers::pi_v<float>;
        }
    }
    // 位置环积分控制速度
    servos[0].set_angle(target_angles[0]);
    servos[1].set_angle(target_angles[1]);

    for (auto& servo : servos) {
        servo.ctrl_mode = motor_servo::CtrlMode::CtrlMode_Angle;
        servo.ctrl();
    }
    // 直接控制速度
    // servos[0].set_speed(speed_y / (2 * numbers::pi_v<float> * r) * 60 - kz*speed_z);
    // servos[1].set_speed(-speed_y / (2 * numbers::pi_v<float> * r) * 60 - kz * speed_z);
    // servos[2].set_speed(speed_y / (2 * numbers::pi_v<float> * r) * 60 - kz * speed_z);
    // servos[3].set_speed(-speed_y / (2 * numbers::pi_v<float> * r) * 60 - kz * speed_z);
    // for (auto& servo : servos) {
    //     servo.ctrl_mode = motor_servo::CtrlMode::CtrlMode_Speed;
    //     servo.ctrl();
    // }
}
